/* Copyright 2017 The Apollo Authors. All Rights Reserved.

Licensed under the Apache License, Version 2.0 (the "License");
you may not use this file except in compliance with the License.
You may obtain a copy of the License at

    http://www.apache.org/licenses/LICENSE-2.0

Unless required by applicable law or agreed to in writing, software
distributed under the License is distributed on an "AS IS" BASIS,
WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
See the License for the specific language governing permissions and
limitations under the License.
=========================================================================*/
#include "modules/map/hdmap/adapter/odr_parser/util_odr_parser.h"

#include <algorithm>
#include <iomanip>
#include <limits>
#include <sstream>
#include <string>
#include <vector>

#include "modules/map/hdmap/adapter/xml_parser/util_xml_parser.h"

#include "modules/map/hdmap/adapter/odr_parser/roadengine_api.h"

#include "modules/map/hdmap/adapter/odr_parser/geometry_util/WorldArcIterator.h"
#include "modules/map/hdmap/adapter/odr_parser/geometry_util/WorldSpiralIterator.h"
#include "modules/common/configs/config_gflags.h"

namespace apollo {
namespace hdmap {
namespace adapter {

// Build: Use map resolution
double UtilOdrParser::CurveResolution = FLAGS_map_resolution;



void UtilOdrParser::BoundaryToCurve(PbCurve* boundary_curve,
                                       const rg::LaneDivRefLine& refLine,
                                       const glm::dvec3& origin, double s, double e,
                                       bool reverse)
{
    PbCurveSegment* curve_segment = boundary_curve->add_segment();
    curve_segment->set_s(s);

    PbLineSegment* line_segment = curve_segment->mutable_line_segment();
    if (reverse) {
        for (int i = (int)refLine.samples.size() - 1; i >= 0; i--) {
            const rg::RefPoint& curSamp = refLine.samples[i];
            //printf("sample point: (%f, %f)\n", curSamp.P[0], curSamp.P[1]);

            PbPoint3D* pt = line_segment->add_point();
            pt->set_x(origin[0] + curSamp.P[0]);
            pt->set_y(origin[1] + curSamp.P[1]);
            pt->set_z(0.0);
            
        }
    }
    else {
        for (size_t i = 0; i < refLine.samples.size(); ++i) {
            const rg::RefPoint& curSamp = refLine.samples[i];
            //printf("sample point: (%f, %f)\n", curSamp.P[0], curSamp.P[1]);

            PbPoint3D* pt = line_segment->add_point();
            pt->set_x(origin[0] + curSamp.P[0]);
            pt->set_y(origin[1] + curSamp.P[1]);
            pt->set_z(origin[2] + 0.0);
            
        }
    }

    // workaround: some map has invalid lane which is very short
    // apollo requries one lane has more than 2 points, if the curve
    // has only 2 points ,add one
    if (refLine.samples.size() == 2) {
      const rg::RefPoint& curSamp = refLine.samples[reverse? 0 : 1];
            //printf("sample point: (%f, %f)\n", curSamp.P[0], curSamp.P[1]);

            PbPoint3D* pt = line_segment->add_point();
            pt->set_x(origin[0] + curSamp.P[0]);
            pt->set_y(origin[1] + curSamp.P[1]);
            pt->set_z(origin[2] + 0.0);
    }

    UtilOdrParser::SetFirstPoint(curve_segment, line_segment);
    curve_segment->set_length(e - s);
}

void UtilOdrParser::CenterToCurve(PbCurve* curve,
                                       const rg::LaneDivRefLine& refLine,
                                       const glm::dvec3& origin, double s, double e,
                                       bool reverse)
{
    // if (!refLine.isDriving) {
    //     AINFO << "Warning: not driving lane when set center curve!";
    // }
    if (refLine.laneMidSamples.size() == 0) {
      printf("Error: no mid samples for ref line.");
    }
    PbCurveSegment* curve_segment = curve->add_segment();
    curve_segment->set_s(s);

    PbLineSegment* line_segment = curve_segment->mutable_line_segment();

    if (reverse) {
        for (int i = (int)refLine.laneMidSamples.size() - 1; i >= 0; i--) {
            const rg::RefPoint& curSamp = refLine.laneMidSamples[i];
        
            PbPoint3D* pt = line_segment->add_point();
            pt->set_x(origin[0] + curSamp.P[0]);
            pt->set_y(origin[1] + curSamp.P[1]);
            pt->set_z(origin[2] + 0.0);
            
        }
    }
    else {
        for (size_t i = 0; i < refLine.laneMidSamples.size(); ++i) {
            const rg::RefPoint& curSamp = refLine.laneMidSamples[i];
        
            PbPoint3D* pt = line_segment->add_point();
            pt->set_x(origin[0] + curSamp.P[0]);
            pt->set_y(origin[1] + curSamp.P[1]);
            pt->set_z(0.0);
            
        }
    }

    // workaround: some map has invalid lane which is very short
    // apollo requries one lane has more than 2 points, if the curve
    // has only 2 points ,add one
    if (refLine.samples.size() == 2) {
      const rg::RefPoint& curSamp = refLine.laneMidSamples[reverse? 0 : 1];
            //printf("sample point: (%f, %f)\n", curSamp.P[0], curSamp.P[1]);

            PbPoint3D* pt = line_segment->add_point();
            pt->set_x(origin[0] + curSamp.P[0]);
            pt->set_y(origin[1] + curSamp.P[1]);
            pt->set_z(origin[2] + 0.0);
    }

    UtilOdrParser::SetFirstPoint(curve_segment, line_segment);
    curve_segment->set_length(e - s);
}

// s: start offset of the curve
// e: end offset of the curve, -1 if reach the whole curve end
// offset: width offset
void UtilOdrParser::ToCurve(const PlanView& planview, double s, double e, double offset,
                           PbCurve* curve, bool reverseXAxis)
{
  CHECK_NOTNULL(curve);

  for (size_t i = 0; i < planview.geometrys.size(); i++) {
    Geometry* geometry = planview.geometrys[i];
    Geometry* next_geometry = (i == planview.geometrys.size()  - 1) ? NULL : planview.geometrys[i + 1];

    if (geometry->s + geometry->length <= s) {
      continue; // have not reach the scope of this curve
    }
    if ( e > 0.0 && geometry->s >= e) {
      break;  // beyond the curve
    }

    PbCurveSegment* curve_segment = curve->add_segment();
    curve_segment->set_s(geometry->s < s ? s : geometry->s);

    PbLineSegment* line_segment = curve_segment->mutable_line_segment();

    double segment_s = 0.0;
    double segment_e = geometry->length;
    if (s > geometry->s) segment_s = s - geometry->s;
    if (next_geometry && e > 0.0 && e < next_geometry->s) segment_e = e - geometry->s;

    SetPointSet(geometry, segment_s, segment_e, offset, line_segment, reverseXAxis);

    SetFirstPoint(curve_segment, line_segment);
    curve_segment->set_length(segment_e - segment_s);
  }
}

void UtilOdrParser::SetPointSet(Geometry* geometry, double s, double e,
                          double offset, PbLineSegment* line_segment,
                          bool reverseXAxis)
{
  double resolution = CurveResolution;
  glm::dmat3x3 uv2xy(
      std::cos(geometry->hdg),	std::sin(geometry->hdg),	0.0,
			-std::sin(geometry->hdg),	std::cos(geometry->hdg),	0.0,
			geometry->x,				      geometry->y,      				1.0
  );

  bool finished = false;
  if (geometry->type == GEO_LINE) {
    glm::dvec3 P0 = glm::dvec3(0.0, -offset, 1.0);
    for (double i = s; ; i += resolution) {
      if (i >= e) {
						i = e;
						finished = true;
			}
      P0.x = i;
      AddPoint(uv2xy, P0, line_segment, reverseXAxis);

      if (finished) break;
    }
  } else if (geometry->type == GEO_PARAPOLY3) {
    ParaPoly3& poly3 = geometry->poly3;
    // every 20cm get a point
    double step = 1.0 / (geometry->length / resolution);
    for (double p = s / geometry->length; 
            ;
            p += step) {
      if (p >= e / geometry->length) {
								p = e / geometry->length;
								finished = true;
			}
      double x = poly3.aU + poly3.bU * p +
                 poly3.cU * p *p +
                 poly3.dU * p * p * p;
      double y = poly3.aV + poly3.bV * p +
                 poly3.cV * p *p +
                 poly3.dV * p * p * p -
                 offset;
      glm::dvec3 P0 = glm::dvec3(x, y, 1.0);
      AddPoint(uv2xy, P0, line_segment, reverseXAxis);

      if (finished) break;
    } 
  }else if (geometry->type == GEO_ARC) {
      apollo::FWorldArcIterator aIt(geometry->arc_curvature,
                            (e - s < resolution) ? resolution : e -s,
                            s, resolution);
      while (aIt.HasNext()) {
        glm::dvec3 P0 = aIt.Sample();
        aIt.Next();
        P0.y = P0.y - offset;
        AddPoint(uv2xy, P0, line_segment, reverseXAxis);
      }

  } else if (geometry->type == GEO_SPIRAL) {
    double s_end = (s - e) < resolution ? s + resolution : e;
    double curve_start = geometry->spiral_curve_start + 
                        (geometry->spiral_curve_end - geometry->spiral_curve_start) * 
                        (s / geometry->length);
    double curve_end = geometry->spiral_curve_start +
                        (geometry->spiral_curve_end - geometry->spiral_curve_start) * 
                        (s_end / geometry->length);
    apollo::FWorldSpiralIterator sIt(curve_start, curve_end, 
                             e - s, resolution);
    while (sIt.HasNext()) {
      glm::dvec3 P0 = sIt.Sample();
      sIt.Next();
      P0.y = P0.y - offset;
      AddPoint(uv2xy, P0, line_segment, reverseXAxis);
    }
  }
}

void UtilOdrParser::SetFirstPoint(PbCurveSegment* curve_segment,
                           PbLineSegment* line_segment)
{

  if (line_segment->point_size() > 0) {
    auto P1 = line_segment->point(0);

    curve_segment->mutable_start_position()->set_x(P1.x());
    curve_segment->mutable_start_position()->set_y(P1.y());
  }
}

void UtilOdrParser::AddPoint(glm::dmat3x3& uv2xy, glm::dvec3& P0, 
                             PbLineSegment* line_segment, bool reverseXAxis)
{
    auto point = uv2xy * P0;

    PbPoint3D* pt = line_segment->add_point();
    pt->set_x(reverseXAxis ? -point.x : point.x);
    pt->set_y(point.y);
    pt->set_z(0.0);
}

void UtilOdrParser::AddPoint(glm::dmat3x3& uv2xy, glm::dvec3& P0,
                             const Lane& lane,
                             const std::vector<Lane*>& lanes_between_center,
                             double cur_s,
                             PbLineSegment* center_line_segment,
                             PbLineSegment* far_line_segment,
                             PbLineSegment* near_line_segment,
                             UtilOdrParser::LaneType lane_type,
                             bool reverseXAxis)
{
    // get distance from boundary to reference line
    double far_offset = 0.0;
    double near_offset = 0.0;

    if (!lane.widths.empty()) {
	      for (size_t li = 0; li < lanes_between_center.size(); li++) {
            near_offset += GetLaneWidth(*lanes_between_center[li], cur_s);
	      }
        far_offset = near_offset + GetLaneWidth(lane, cur_s);
    } else {
        // border
        if (lanes_between_center.size() > 0) {
            near_offset = GetLaneBorder(*lanes_between_center[0], cur_s);
        }
        far_offset = near_offset + GetLaneBorder(lane, cur_s);
    }	  
    if (lane_type == UtilOdrParser::LEFT_LANE) {
      near_offset = -near_offset;
      far_offset = -far_offset;
    }

    glm::dvec3 P1 = P0;
    P1.y = P0.y - near_offset;
    AddPoint(uv2xy, P1, near_line_segment, reverseXAxis);

    P1.y = P0.y - (near_offset + far_offset) / 2.0;
    AddPoint(uv2xy, P1, center_line_segment, reverseXAxis);

    P1.y = P0.y - far_offset;
    AddPoint(uv2xy, P1, far_line_segment, reverseXAxis);
}

// get width of lane at offset 's'
double UtilOdrParser::GetLaneWidth(const Lane& lane, double s)
{
  if (lane.widths.empty()) {
    return 0.0;
  }

  // one lane may have multiple width definition
  // get the width element contains 's'
  // sOffset is started from 0 in each laneSection
  LaneWidth width;
  for (size_t i = 0; i < lane.widths.size(); i++)
  {
    if (i == lane.widths.size() - 1 || lane.widths[i + 1].sOffset > s) {
      width = lane.widths[i];
      break;
    }
  }

  // get offset in this width element
  double ds = s - width.sOffset;
  if (ds < 0.0) {
    printf("ds is below zero: %f\n", ds);
  }

  // get width
  return width.a + width.b * ds + width.c * ds * ds + width.d * ds * ds * ds;
}

double UtilOdrParser::GetLaneBorder(const Lane& lane, double s)
{
  if (lane.borders.empty()) {
    return 0.0;
  }

  // one lane may have multiple width definition
  // get the width element contains 's'
  // sOffset is started from 0 in each laneSection
  LaneWidth width;
  for (size_t i = 0; i < lane.borders.size(); i++)
  {
    if (i == lane.borders.size() - 1 || lane.borders[i + 1].sOffset > s) {
      width = lane.borders[i];
      break;
    }
  }

  // get offset in this width element
  double ds = s - width.sOffset;
  if (ds < 0.0) {
    printf("ERROR: ds is below zero: %f=============\n", ds);
  }

  // get width
  return width.a + width.b * ds + width.c * ds * ds + width.d * ds * ds * ds;
}

ESpeedUnitType UtilOdrParser::ToSpeedUnit(const std::string& unit)
{
   std::string upper_str = UtilXmlParser::ToUpper(unit);
   if (unit == "KM/H")
     return KM_PER_H;
   else if (unit == "M/S")
     return M_PER_S;
   else if (unit == "MPH") {
     return MILE_PER_H;
   }

  return KM_PER_H;
}


void UtilOdrParser::ToStopLineCurve(const PlanView& planview, double s, 
	                                double u0, double u1,
									                PbCurveSegment* curve_segment,
									                bool reverseXAxis)
{
  if (u0 > u1) {
    double tmp = u0;
    u0 = u1;
    u1 = tmp;
  }

  for (auto i = planview.geometrys.size() - 1; i >= 0; i--) {
    Geometry* geometry = planview.geometrys[i];

    if (geometry->s < s) {
      PbLineSegment* line_segment = curve_segment->mutable_line_segment();

      SetPointSetByOffset(geometry, s - geometry->s, u0, u1, line_segment, reverseXAxis);

      SetFirstPoint(curve_segment, line_segment);
      curve_segment->set_length(u1 - u0);
      break;
    }
  }
}                                  

void UtilOdrParser::SetPointSetByOffset(Geometry* geometry, double s, 
                          double u0, double u1, PbLineSegment* line_segment,
                          bool reverseXAxis)
{
  double resolution = CurveResolution;
  glm::dmat3x3 uv2xy(
      std::cos(geometry->hdg),	std::sin(geometry->hdg),	0.0,
			-std::sin(geometry->hdg),	std::cos(geometry->hdg),	0.0,
			geometry->x,				      geometry->y,      				1.0
  );

  if (geometry->type == GEO_LINE) {
      glm::dvec3 P0 = glm::dvec3(0.0, 0.0, 1.0);
      P0.x = s;
      for (double u = u0; u < u1; u += resolution) {
        P0.y = u;
        AddPoint(uv2xy, P0, line_segment, reverseXAxis);
      }
  } else if (geometry->type == GEO_PARAPOLY3) {
    ParaPoly3& poly3 = geometry->poly3;
    // every 20cm get a point
    double p = s / geometry->length;
    double x = poly3.aU + poly3.bU * p +
                 poly3.cU * p *p +
                 poly3.dU * p * p * p;
    double y = poly3.aV + poly3.bV * p +
                 poly3.cV * p *p +
                 poly3.dV * p * p * p;
    for (double u = u0; u < u1; u += resolution) {
      glm::dvec3 P0 = glm::dvec3(x, y - u, 1.0);
      AddPoint(uv2xy, P0, line_segment, reverseXAxis);
    } 
  }else if (geometry->type == GEO_ARC) {
      apollo::FWorldArcIterator aIt(geometry->arc_curvature, 1.0 , s, resolution);
      if (aIt.HasNext()) {
        glm::dvec3 P0 = aIt.Sample();
        for (double u = u0; u < u1; u += resolution) {
          P0.y = P0.y - u;
          AddPoint(uv2xy, P0, line_segment, reverseXAxis);
        }
      }

  } else if (geometry->type == GEO_SPIRAL) {
    double curve_start = geometry->spiral_curve_start + 
                        (geometry->spiral_curve_end - geometry->spiral_curve_start) * 
                        (s / geometry->length);
  double curve_end = geometry->spiral_curve_start +
                        (geometry->spiral_curve_end - geometry->spiral_curve_start) * 
                        ((s + 1.0) / geometry->length);
    apollo::FWorldSpiralIterator sIt(curve_start, curve_end, 
                             1.0, resolution);
    if (sIt.HasNext()) {
      glm::dvec3 P0 = sIt.Sample();
      for (double u = u0; u < u1; u += resolution) {
        P0.y = P0.y - u;
        AddPoint(uv2xy, P0, line_segment, reverseXAxis);
      }
    }
  }
}
void UtilOdrParser::ST2XY(const PlanView& planview, double s, double t, double& rx, double& ry)
{
  for (auto i = 0; i != planview.geometrys.size(); i++) {
    Geometry* geometry = planview.geometrys[i];
    
    if (geometry->s < s) {
      
      glm::dmat3x3 uv2xy(std::cos(geometry->hdg), std::sin(geometry->hdg), 0.0,
                         -std::sin(geometry->hdg), std::cos(geometry->hdg), 0.0,
                         geometry->x, geometry->y, 1.0);

      if (geometry->type == GEO_LINE) {
        glm::dvec3 P0 = glm::dvec3(0.0, 0.0, 1.0);
        P0.x = s;
        P0.y = t;
        auto point = uv2xy * P0;
        rx = point.x;
        ry = point.y;

      } else if (geometry->type == GEO_PARAPOLY3) {
        ParaPoly3& poly3 = geometry->poly3;
        double p = (s-geometry->s) / geometry->length;
        double x =
            poly3.aU + poly3.bU * p + poly3.cU * p * p + poly3.dU * p * p * p;
        double y =
            poly3.aV + poly3.bV * p + poly3.cV * p * p + poly3.dV * p * p * p;
        glm::dvec3 P0 = glm::dvec3(x, y - t, 1.0);
        auto point = uv2xy * P0;
        rx = point.x;
        ry = point.y;
      } else if (geometry->type == GEO_ARC) {
        apollo::FWorldArcIterator aIt(geometry->arc_curvature, 1.0, s-geometry->s,
                                      UtilOdrParser::CurveResolution);
        if (aIt.HasNext()) {
          glm::dvec3 P0 = aIt.Sample();
          P0.y = P0.y - t;
          auto point = uv2xy * P0;
          rx = point.x;
          ry = point.y;
        }

      } else if (geometry->type == GEO_SPIRAL) {
        double curve_start =
            geometry->spiral_curve_start +
            (geometry->spiral_curve_end - geometry->spiral_curve_start) *
                ((s-geometry->s) / geometry->length);
        double curve_end =
            geometry->spiral_curve_start +
            (geometry->spiral_curve_end - geometry->spiral_curve_start) *
                ((s + 1.0) / geometry->length);
        apollo::FWorldSpiralIterator sIt(curve_start, curve_end, 1.0,
                                         UtilOdrParser::CurveResolution);
        if (sIt.HasNext()) {
          glm::dvec3 P0 = sIt.Sample();
          P0.y = P0.y - t;
          auto point = uv2xy * P0;
          rx = point.x;
          ry = point.y;
        }
      }
      break;
    }
  }
}

void UtilOdrParser::cornerLocalToGlobal(const ObjectOutline& outline, double hdg, double x, double y, 
	                           PbPolygon* polygon)
{
  glm::dmat3x3 uv2xy(
    std::cos(hdg),	std::sin(hdg),	0.0,
    -std::sin(hdg),	std::cos(hdg),	0.0,
    x,				      y,      				1.0
  );
  for(auto cornerlocal : outline.cornerLocals)
  {
    //glm::dvec3 P0 = glm::dvec3(cornerlocal.v, cornerlocal.u * -1, 1);
    glm::dvec3 P0 = glm::dvec3(cornerlocal.u*-1, cornerlocal.v*-1 , 1);
    auto point = uv2xy * P0;
    PbPoint3D* pt = polygon->add_point();
    pt->set_x(point.x);
    pt->set_y(point.y);
    pt->set_z(point.z);
  }
}   
void UtilOdrParser::ToLightPolygon(const PlanView& planview, double s, 
	                           PbPolygon* polygon,
									           bool reverseXAxis)
{
  for (size_t i = planview.geometrys.size() - 1; i >= 0; i--) {
    Geometry* geometry = planview.geometrys[i];

    if (geometry->s < s) {
      glm::dmat3x3 uv2xy(
          std::cos(geometry->hdg),	std::sin(geometry->hdg),	0.0,
          -std::sin(geometry->hdg),	std::cos(geometry->hdg),	0.0,
          geometry->x,				      geometry->y,      				1.0
      );
      glm::dvec3 P0 = glm::dvec3(s - geometry->s, 0.0, 1.0);
      AddTrafficLightPolygonPoints(uv2xy, P0, polygon, reverseXAxis);
      break;
    }
  }
  
}                             

void UtilOdrParser::AddTrafficLightPolygonPoints(const glm::dmat3x3& uv2xy,
                                  const glm::dvec3& P0, 
                                  PbPolygon* polygon,
                                  bool reverseXAxis)
{
  glm::dvec3 P1(P0.x, P0.y + 4, 1.0);
  glm::dvec3 P2(P0.x, P0.y - 10, 1.0);
  auto p1 = uv2xy * P1;
  auto p2 = uv2xy * P2;
  PbPoint3D* pt = polygon->add_point();
  pt->set_x(reverseXAxis ? -p1.x : p1.x);
  pt->set_y(p1.y);
  pt->set_z(-2);

  pt = polygon->add_point();
  pt->set_x(reverseXAxis ? -p2.x : p2.x);
  pt->set_y(p2.y);
  pt->set_z(-2);

  pt = polygon->add_point();
  pt->set_x(reverseXAxis ? -p2.x : p2.x);
  pt->set_y(p2.y);
  pt->set_z(8);

  pt = polygon->add_point();
  pt->set_x(reverseXAxis ? -p1.x : p1.x);
  pt->set_y(p1.y);
  pt->set_z(8);
}                          

Road* UtilOdrParser::FindRoad(const std::map<std::string, Road*>& roads,
                          const std::string& roadId)
{
  auto rit = roads.find(roadId);
  if (rit == roads.end()) {
    AERROR<<"Error when process connection link.";
    return NULL;
  }

  return rit->second;
}

const LaneInternal* UtilOdrParser::GetLaneInRoad(const RoadInternal* road_internal, const std::string& lane_id)
{
  for (size_t i = 0; i < road_internal->sections.size(); i++) {
    const std::vector<LaneInternal>& lanes = road_internal->sections[i].lanes;
    for (size_t li = 0; li < lanes.size(); li++) {
      const LaneInternal& lane_internal = lanes[li];
      if (lane_internal.lane.id().id() == lane_id) {
        return &lane_internal;
      }
    }
  }

  return nullptr;
}

apollo::common::PointENU UtilOdrParser::GetStartPointOfLane(const LaneInternal* lane_internal,
                                          UtilOdrParser::LaneBoundaryType side)
{
  const PbCurve& curve = GetLaneCurve(lane_internal, side);

  return curve.segment(0).start_position();
}

apollo::common::PointENU UtilOdrParser::GetEndPointOfLane(
                                            const LaneInternal* lane_internal,
                                            UtilOdrParser::LaneBoundaryType side)
{
  const PbCurve& curve = GetLaneCurve(lane_internal, side);

  const PbCurveSegment& curve_segment = curve.segment(curve.segment_size()-1);
  const PbLineSegment& line_segment = curve_segment.line_segment();

  const PbPoint3D& pt = line_segment.point(line_segment.point_size() -1);

  apollo::common::PointENU point;
  point.set_x(pt.x());
  point.set_y(pt.y());
  point.set_z(pt.z());

  return point;
}

const PbCurve& UtilOdrParser::GetLaneCurve(const LaneInternal* lane_internal,
                                       UtilOdrParser::LaneBoundaryType side)
{
  if (side == UtilOdrParser::LEFT_BOUNDARY)
      return lane_internal->lane.left_boundary().curve();
  else if (side == UtilOdrParser::RIGHT_BOUNDARY)
      return lane_internal->lane.right_boundary().curve();
  else //if (side == UtilOdrParser::CENTER)
      return lane_internal->lane.central_curve();

}

void UtilOdrParser::MegerPointsInSameLine(const std::vector<apollo::common::PointENU>& sorted_points,
                                          std::vector<apollo::common::PointENU>& merged_points)
{
  size_t i;
  for (i = 0; i < sorted_points.size() - 2; ) {
    ADEBUG << "point: " << (int)i;
    // in our scenirio, only four points can be in same line,
    // so check four points every time
    if (PointsInSameLine(sorted_points, i, i + 3)) {
      // discard points in middle
      merged_points.push_back(sorted_points[i]);
      merged_points.push_back(sorted_points[i + 3]);
      i += 4;
    } else {
      merged_points.push_back(sorted_points[i]);
      merged_points.push_back(sorted_points[i + 1]);
      i += 2;
    }
  }

  if (i == sorted_points.size() - 2) {
    merged_points.push_back(sorted_points[i]);
    merged_points.push_back(sorted_points[i + 1]);
  }
}

// check if points is in same line
// param: first - the index of the first point
// param: end - the index of the end point
// then check points between first and end are in the line from first to end
//
// refer to: blog.sina.com.cn/s/blog_5d5c80840101bnhw.html
bool UtilOdrParser::PointsInSameLine(const std::vector<apollo::common::PointENU>& points,
                                     size_t first, size_t end)
{
  // the threshold seems too short!!!
  const double threshold = 0.4; // unit: m

  const apollo::common::PointENU& p1 = points[first];
  const apollo::common::PointENU& p2 = points[end];
  double distance;
  for (size_t i = first + 1; i < end; i++) {
    const apollo::common::PointENU& p = points[i];

    double A = p2.y() - p1.y();
    double B = p1.x() - p2.x();
    double C = p2.x() * p1.y() - p1.x() * p2.y();
    distance = fabs(A * p.x() + B * p.y() + C ) / 
                          sqrt( A * A + B * B);

    ADEBUG << "distance of "<< (int)i << "is: " << distance;
    if (distance > threshold) {
      break;
    }
  }

  return distance < threshold;
}
/*
// distance to the line segment
bool UtjilOdrParser::DistanceToSegment(const std::vector<apollo::common::PointENU>& points,
                                     size_t first, size_t end)
{                                     
  // the threshold seems too short!!!
  const double threshold = 0.2; // unit: m

  const apollo::common::PointENU& p1 = points[first];
  const apollo::common::PointENU& p2 = points[end];
  double distance;
  for (size_t i = first + 1; i < end; i++) {
    const apollo::common::PointENU& p = points[i];

    double cross = (p2.x() - p1.x()) * (p.x() - p1.x()) + (p2.y() - p1.y()) * (p.y() - p1.y());

    if (cross <= 0) {
      distance = std::sqrt((p.x() - p1.x()) * (p.x() - p1.x()) + (p.y() - p1.y()) * (p.y() - p1.y()));
    } else {
      double d2 = (p2.x() - p1.x()) * (p2.x() - p1.x()) + (p2.y() - p1.y()) * (p2.y() - p1.y());

      if (cross >= d2) {
        distance = std::sqrt((p.x() - p2.x()) * (p.x() - p2.x()) + (p.y() - p2.y()) * (p.y() - p2.y()));
      } else {
        double r = cross / d2;
        double px = p1.x() + (p2.x() - p1.x()) * r;
        double py = p1.y() + (p2.y() - p1.y()) * r;

        distance = std::sqrt((p.x() - px) * (p.x() - px) + (py - p1.y()) * (py - p1.y()));
      }
    }

    printf("distance of %d is: %f\n", i, distance);

    if (distance > threshold) {
      break;
    }
  }

  return distance < threshold;
}  
*/                                                                           

}  // namespace adapter
}  // namespace hdmap
}  // namespace apollo
